package org.scalpel.mainApps;


import ioio.lib.api.exception.ConnectionLostException;

import java.util.HashMap;
import java.util.concurrent.ArrayBlockingQueue;
import java.util.concurrent.ScheduledThreadPoolExecutor;

import org.scalpel.HHObjects.HHObjectInspector;
import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;
import org.scalpel.mainApps.R;
import org.scalpel.mainApps.HHObjects.Jack;
import org.scalpel.mainApps.HHObjects.ModLoco;
import org.scalpel.mainApps.HHObjects.Sharp;
import org.scalpel.mainApps.HHObjects.Trajectory;

import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.view.MotionEvent;
import android.util.Log;

/**
 * @author Jerome Monceaux, Yvanne Paviet-Salomon, Xuelan Sun
 * @version 1.0
 * !!!! attention setPid provoque un pic qui peut le faire planter....
 * mettre sleep de 500 pour compenser et soulager le code (stabilite) -- cf sharp and 3e point de sortie
 **/

public class Strategy extends HHObject {
	private static final String TAG="Strategy";

	private double fDefaultDistPrecision = 0.02; 
	private double fDefaultAnglePrecision = 0.02; 
	private ModLoco fModLoco = null;
	private Sharp fSharp10 = null;
	private Sharp fSharp30 = null;
	private Sharp fSharp50 = null;
	private Sharp fSharp70 = null;
	private double fSpeedP;
	private double fSpeedI;
	private double fSpeedD;
	private double fDistanceP;
	private double fDistanceI;
	private double fDistanceD;
	private double fAngleP;
	private double fAngleI;
	private double fAngleD;
	private Jack fJack = null;
	private boolean  pass = false;
	private boolean  pass2 = false;
	private boolean  pass3 = false;
	private boolean  pass4 = false;
	private boolean fback = false;
	private boolean fcontinue = false;
	private boolean[] flosePoint1= new boolean[50]; 
	private boolean[] flosePoint2= new boolean[50]; 
	//Declaration des timers
	
	private  Thread[]  t=new Thread[170];
	private Thread timer;
	private  int[] Time = new int[50];	
	//Methodes
	
	// 3 Threads spécials 
	Thread tBouteille = new Thread() {
//Thread bouteille		
		public void run(){;

			try {
					Thread.sleep(1000,0); /*why only const*/
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
		}
			
		};
	
		Thread tBouteille2 = new Thread() {
			//Thread bouteille		
					public void run(){;

						try {
								Thread.sleep(1000,0); /*why only const*/
								} catch (InterruptedException e) {
									// TODO Auto-generated catch block
									e.printStackTrace();
								}
					}
						
					};
		Thread tRotationBouteille1= new Thread() {
			
			public void run(){;

				try {
						Thread.sleep(1000,0); 
						} catch (InterruptedException e) {
							// TODO Auto-generated catch block
							e.printStackTrace();
						}
			}
				
			};
			
			Thread tRotationBouteille2= new Thread() {
				
				public void run(){;

					try {
							Thread.sleep(1000,0); 
							} catch (InterruptedException e) {
								// TODO Auto-generated catch block
								e.printStackTrace();
							}
				}
					
				};
	
		
		
		
	private void mySleep( int ms)
	{
		try {
			Thread.sleep(ms, 0);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public Strategy(String fName) {
		super(fName);
	}

	@Override
	public void finish() {
		// TODO Auto-generated method stub

	}

	HHObjectInspector  fInspector = null;

	public void setInspector( HHObjectInspector inspector )
	{
		fInspector = inspector;
	}

	private void initConfig() {
		fDistanceP = 6.0;
		fDistanceI = 1.0;
		fDistanceD = 0.1;
		fModLoco.setDistancePid(fDistanceP, fDistanceI, fDistanceD);
		fAngleP = 0.5;
		fAngleI = 0.3;
		fAngleD = 0.1;
		fModLoco.setAnglePid(fAngleP, fAngleI, fAngleD);
		fSpeedP = 0.71;
		fSpeedI = 0.15;
		fSpeedD = 0.;
		fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
		fModLoco.fLocoEngine.setUTurnAvoidance(true); /* mettre à true si on veut mettre le reverse*/
		fModLoco.fLocoEngine.emergencyUnfreeze();
		fModLoco.fLocoEngine.setMaxSpeed(1.5);
		fModLoco.fLocoEngine.setMirror(false); /**Mettre à true si on veut changer de coter*/
		fModLoco.fLocoEngine.setAnglePrecision(0.4);
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		//thread special
		 timer = new Thread() {
			public void run(){
				mySleep(8000);
			}
		};
		for(int k=0; k<160;k++){

		//	myTimer(t[i],Time[i]);
			t[k] = new Thread() {
					public void run(){;
	
						try {
								Thread.sleep(5000,0); /*why only const*/
								} catch (InterruptedException e) {
									// TODO Auto-generated catch block
									e.printStackTrace();
								}
					}
						
					};
			
		}
	
		for(int b=0; b<45;b++)
		{
		  flosePoint1[b] = false  ;
		}
		
		for(int b=0; b<45;b++)
		{
		  flosePoint2[b] = false  ;
		}
	}
	

	private void Point0() throws InterruptedException, ConnectionLostException
	{
		/**0e point*/
		fModLoco.setTarget(1, 0.25, 0);

		while(fModLoco.fLocoEngine.isTargetReached() != true)
		{
			mySleep(100);
		}
		mySleep(200);
	}
	/*mettre || dans le while pour faire entierement la methode*/
	private void Point1() throws InterruptedException, ConnectionLostException
	{
		
		/**FIRST point*/
		fModLoco.setTarget(1.55, 0.30, 0);
		pass2 = false;
		pass = false;
		fback=false;	
		timer.start();
		
		while(fModLoco.fLocoEngine.isTargetReached() != true  && fcontinue != true)
		{
			if (fback == false && flosePoint2[1] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint2[1] == false)){checkObstacleBack();}
			mySleep(500);
			
			if(!timer.isAlive() && pass == false)
			{
				fModLoco.fLocoEngine.setMaxSpeed(1);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setTarget(1.1, 0.35, 0);
				fback = true;
				pass = true;
				flosePoint1[1] = true;
				t[100].start();
			}
			
			if(!t[100].isAlive() && pass2 == false && pass == true)
			{
				flosePoint2[1] = true;
				flosePoint1[1] = false;
				fModLoco.setTarget(1.55, 0.4, 0);
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				pass2 = true;
				t[101].start();
			}
			
			if(!t[101].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(200);
	}
	private void Point2() throws InterruptedException, ConnectionLostException
	{
		/**SECOND point*/
		fcontinue = false;
		pass2 = false;
		pass = false;
		fback=false;	
		pass3 = false;
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.setTarget(0., 0., -1.57);
		t[2].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true  && fcontinue != true)
		{
			if ( flosePoint1[2] ==false ){checkObstacleFront();}
			
			mySleep(300);
			
				if(!t[2].isAlive() && pass == false)
				{
					fModLoco.setTarget(1.55, 0.670, 0);	
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					pass = true;
					pass2 = true; /*!!!!*/
					t[103].start();
					flosePoint1[2] = true;
					flosePoint2[1] = true;
					//t[102].start();
				}
				
				/*if(!t[102].isAlive() && pass2 == false && pass == true)
				{
					fModLoco.setTarget(1, 0.55, 0);
					pass2 = true;
					t[103].start();
					flosePoint1[2] = true;
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				}*/
				
				if(!t[103].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}

		} mySleep(400);
	}
	
	private void Point3() throws InterruptedException, ConnectionLostException
	{
		/**3e point*/
		fModLoco.setTarget(1.55, 0.670, 0);
		fcontinue = false;
		pass2 = false;
		pass = false;
		fback=false;	
		flosePoint2[1] = true;
		t[3].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (flosePoint1[3] == false){checkObstacleFront();}
			
			mySleep(300);
			
			if(!t[3].isAlive() && pass == false)
			{
				fModLoco.setTarget(1.117, 0.53, 0);
				pass = true;
				t[104].start();
			}
			
			if(!t[104].isAlive() && pass2 == false && pass == true)
			{
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				fModLoco.setTarget(1.55, 0.25,0);
				flosePoint1[3] = true;
				pass2 = true;
				t[105].start();
			}
			if(!t[105].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(300);
	}

	private void Point4() throws InterruptedException, ConnectionLostException
	{
		/**4e point*/
		fModLoco.fLocoEngine.setMaxSpeed(1.5);
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.setTarget(0, 0, -3.14);
		fcontinue = false;
		pass = false;
		pass2 = false;
		t[4].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (flosePoint1[4] == false){checkObstacleFront();}
				
				if(!t[4].isAlive() && pass == false)
				{
					fModLoco.fLocoEngine.setUTurnAvoidance(true);
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(1.117, 0.63, 0);
					flosePoint1[4] = true;
					pass = true;
					t[106].start();
				}
				if(!t[106].isAlive() && pass2 == false && pass == true)
				{
					
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(1.55, 0.25,0);
					flosePoint1[4] = true;
					pass2 = true;
					t[107].start();
				}
				if(!t[107].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}
				mySleep(300);
		}
		mySleep(400);
	}
	
	private void Point5() throws InterruptedException, ConnectionLostException
	{
		/**5e point*/
		fModLoco.setTarget(1.117, 0.63, 0);
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		t[5].start();
		
		
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[5] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[5] == false)){checkObstacleBack();}
			
			mySleep(300);
			
			
			if(!t[5].isAlive() && pass == false)
		   		{	
					fModLoco.setTarget(1.55, 0.25, 0);
					fModLoco.fLocoEngine.setUTurnAvoidance(true);
					t[108].start();
					pass = true;
					fback = true;
				}
			if(!t[108].isAlive() && pass2 == false && pass == true)
			{
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				fModLoco.setTarget(1., 0.5,0);
				flosePoint1[5] = true;
				pass2 = true;
				t[109].start();
			}
			if(!t[109].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(500);
	}
	private void Point6() throws InterruptedException, ConnectionLostException
	{
		/**6e point*/
		t[6].start();
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.fLocoEngine.setMaxSpeed(1);
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.setTarget(0.8, 0.75, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(300);
		
			if (fback == false && flosePoint1[6] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[6] == false)){checkObstacleBack();}
			
			if(!t[6].isAlive() && pass == false)
	   		{	
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setTarget(1.4, 0.35,0);
				pass = true;
				fback = true;
				t[110].start();
			}
			if(!t[110].isAlive() && pass2 == false && pass == true)
			{
				fModLoco.setTarget(0.9,0.6,0);
				pass2 = true;
				t[111].start();
			}
			if(!t[111].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(600);
	}
	private void Point7() throws InterruptedException, ConnectionLostException
	{
		/**7e point*/
		t[7].start();
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.fLocoEngine.setMaxSpeed(1.3);
		fModLoco.fLocoEngine.setDistancePrecision(0.035);
		fModLoco.setTarget(0.35, 0.85, 0);
		while((fModLoco.fLocoEngine.isTargetReached() != true )&&( fcontinue != true))
		{
			
			if ((fback == false) && (flosePoint1[7] == false) ){checkObstacleFront();}
			if ((fback == true) && (flosePoint1[7] == false) ){checkObstacleBack();}
				mySleep(300);
				if(!t[7].isAlive() && pass == false)
		   		{	
					fModLoco.fLocoEngine.setUTurnAvoidance(true);
					fModLoco.setTarget(1.1, 0.65, 0);
					fModLoco.fLocoEngine.setDistancePrecision(0.001);
					pass = true;
					fback = true;
					t[112].start();
				}
				if(!t[112].isAlive() && pass2 == false && pass == true) 
		   		{	
					fModLoco.setTarget(0.35, 0.8, 0);
					fModLoco.fLocoEngine.setDistancePrecision(0.03);
					//t[113].start();
					fback = false;
					pass2 = true;
				}

				/*if(!t[113].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}*/
		}
		mySleep(400);
	}
	
	private void Point8() throws InterruptedException, ConnectionLostException
	{
		/**8e point*/
		t[8].start();
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=true;	
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.fLocoEngine.setMaxSpeed(1.3);
		fModLoco.fLocoEngine.setUTurnAvoidance(true);
		fModLoco.setTarget(0.60, 0.95, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if ((fback == false) && (flosePoint1[8] == false) ){checkObstacleFront();}
			if ((fback == true) && (flosePoint1[8] == false) ){checkObstacleBack();}
							mySleep(300);

				if(!t[8].isAlive() && pass == false)
				{
					fModLoco.setTarget(0.35, 0.8, 0);
					fModLoco.fLocoEngine.setDistancePrecision(0.001);
					pass = true;
					fback = false;
					t[114].start();
				}
				if(!t[114].isAlive() && pass2 == false && pass == true)
				{
					fModLoco.setTarget(0.45,0.95,0);
					flosePoint1[8] = true;
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					pass2 = true;
					t[115].start();
				}
				if(!t[115].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
					
				}
		}
		mySleep(200);
	}
	
	private void Point9() throws InterruptedException, ConnectionLostException
	{
		/**9e point*/
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.fLocoEngine.setMaxSpeed(1.5);
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.setTarget(0, 0, -1.57);
		pass3=false;
		t[80].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(100);
			if(!t[80].isAlive() && pass3 == false  )
			{
				fcontinue = true;
				
			}
		}
		mySleep(400);
	}
	
	private void Point10() throws InterruptedException, ConnectionLostException
	{
		/**10e point*/
		t[10].start();
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.setTarget(0.75, 1.65, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(300);
			if ((fback == false) && (flosePoint1[10] == false) ){checkObstacleFront();}
			if ((fback == true) && (flosePoint1[10] == false) ){checkObstacleBack();}
		
		if(!t[10].isAlive() && pass == false)
   		{	
			fModLoco.fLocoEngine.setUTurnAvoidance(true);
			fModLoco.setTarget(0.75, 0.90,0);
			fModLoco.fLocoEngine.setMaxSpeed(1.);
			pass = true;
			fback = true;
			t[116].start();
		}
		if(!t[116].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
			fModLoco.setTarget(0.75, 1.4,0);
			flosePoint1[10] = true;
			pass2 = true;
			t[117].start();
		}
		if(!t[117].isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fcontinue = true;
		}
		mySleep(200);
	}
	}	
	private void Pointrot() throws InterruptedException, ConnectionLostException
	{
		pass = false;
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.setTarget(0., 0., -1.57);
		fModLoco.fLocoEngine.setAnglePrecision(0.1);
		tRotationBouteille1.start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(300);
			
			if(!tRotationBouteille1.isAlive() && pass == false)
			{
				fcontinue = true;
				pass = true;
			}
	}
	}
	private void PointAvant_bouteille() throws InterruptedException, ConnectionLostException
	{
		/**10e point*/
		t[70].start();

		fModLoco.setTarget(0.75, 1.87, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(300);
			
		if(!t[70].isAlive() &&  pass == true)
		{
			fcontinue = true;
		}
		mySleep(200);
	}
	}
	private void Pointrot2() throws InterruptedException, ConnectionLostException
	{
		fModLoco.fLocoEngine.setUTurnAvoidance(false);

		fModLoco.setTarget(0., 0., -1.57);
		tRotationBouteille2.start();
	
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(300);
			if(!tRotationBouteille2.isAlive() && pass == false)
			{
				fcontinue = true;
				pass = true;
			}
	}
	
	}

	private void Pointbouteille() throws InterruptedException, ConnectionLostException
	{
		/**10e point*/
		t[60].start();
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;			
		fModLoco.fLocoEngine.setMaxSpeed(1.5);
		fModLoco.fLocoEngine.setDistancePrecision(0.001);
		fModLoco.setTarget(0.75, 2, 0);
		pass3 = false ;
		tBouteille.start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(300);
			if ((fback == true) && (flosePoint1[30] == false) ){checkObstacleBack();}
		
		if(!tBouteille.isAlive() && pass == false)
   		{	
			fModLoco.fLocoEngine.setUTurnAvoidance(true);
			fModLoco.fLocoEngine.setDistancePrecision(0.03);
			fModLoco.setTarget(0.75, 1.50,0);
			pass = true;
			fback = true;
			t[61].start();
		}
		if(!t[61].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setTarget(0.75, 2, 0);
			fModLoco.fLocoEngine.setDistancePrecision(0.001);
			flosePoint1[30] = true;
			pass2 = true;
			tBouteille2.start();
		}
		if(!tBouteille2.isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fModLoco.fLocoEngine.setDistancePrecision(0.03);
			fModLoco.setTarget(0.75, 1.50,0);
			pass3 = true;
			fback = true;
		}
		
		mySleep(200);
	
	}
	}
	
	private void Point11() throws InterruptedException, ConnectionLostException
	{
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		/**11e point*/
		fModLoco.fLocoEngine.setMaxSpeed(1.2);
		fModLoco.fLocoEngine.setAnglePrecision(0.3);
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.setTarget(0,0 , 0.001);
		
		//t[11].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			
		
			mySleep(300);			/*if (fback == false && flosePoint1[11] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[11] == false))checkObstacleBack();
			
mySleep(100);

			if(!t[11].isAlive() && pass == false)
			{				
				fModLoco.setTarget(0.55, 1.550, 0);
				fModLoco.fLocoEngine.setMaxSpeed(1.);
				fModLoco.fLocoEngine.setDistancePrecision(0.03);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fback = true;
				pass = true;
				t[118].start();
			}
			if(!t[118].isAlive() && pass2 == false && pass == true)
			{
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				fModLoco.setTarget(0.65, 1.35,0);
				flosePoint1[5] = true;
				pass2 = false;
				flosePoint1[11] = true;
				t[119].start();
			}
			if(!t[119].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(200);*/
	}
	}
	
	private void Point12() throws InterruptedException, ConnectionLostException
	{
		/**12e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.fLocoEngine.setUTurnAvoidance(true);
		fModLoco.fLocoEngine.setMaxSpeed(1.5);
		fModLoco.setTarget(1.583, 1.638, 0);
		t[12].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[12] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[12] == false))checkObstacleBack();
			mySleep(300);
			
				
				if(!t[12].isAlive() && pass == false)
				{
					fModLoco.fLocoEngine.setDistancePrecision(0.001);
					fModLoco.setTarget(0.65, 1.650, 0);
					pass = true;
					t[120].start();
					fback=true;
					
				}
				if(!t[120].isAlive() && pass2 == false && pass == true)
				{
					//fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(1.583, 1.638, 0);
					//flosePoint1[12] = true;
					pass2 = true;
					fback=false;
					t[121].start();
				}
				if(!t[121].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}
		}mySleep(200);
	}
	
	private void Point13() throws InterruptedException, ConnectionLostException
	{
		/**13e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.setTarget(1.586, 1.410, 0);
		fModLoco.fLocoEngine.setMaxSpeed(1.);
		t[13].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			
			if (fback == false && flosePoint1[13] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[13] == false))checkObstacleBack();
				mySleep(200);
				if(!t[13].isAlive() && pass == false)
				{				
					fModLoco.setTarget(1.486, 1.640, 0);
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					flosePoint1[13] = true;
					pass = true;
					//t[122].start();
				}
				/*if(!t[122].isAlive() && pass2 == false && pass == true)
				{
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(0.65, 1.56,0);
					flosePoint1[13] = true;
					pass2 = true;
					t[123].start();
				}*/
				if(!t[123].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}
			
		}
			mySleep(200);
	}
	
	private void Point14() throws InterruptedException, ConnectionLostException
	{
		/**14e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.setTarget(1.418, 1.623, 0);
		
		t[14].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(100);
			
			if (fback == false && flosePoint1[14] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[14] == false))checkObstacleBack();
				
				if(!t[14].isAlive() && pass == false)
				{				
					fModLoco.setTarget(1.16, 1.40, 0);
					fModLoco.fLocoEngine.setMaxSpeed(1.);
					fModLoco.fLocoEngine.setUTurnAvoidance(true);
					fback = true;
					pass = true;
					t[124].start();
				}
				if(!t[124].isAlive() && pass2 == false && pass == true)
				{
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(1., 0.5,0);
					flosePoint1[14] = true;
					pass2 = true;
					t[125].start();
				}
				if(!t[125].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}
		}
		mySleep(400);
	}
	
	private void Point15() throws InterruptedException, ConnectionLostException
	{
		/**15e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.setTarget(1.851, 1.583, 0);
		t[15].start();
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(100);
			
			if (fback == false && flosePoint1[15] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[15] == false))checkObstacleBack();
				
				if(!t[15].isAlive() && pass == false)
				{				
					fModLoco.setTarget(1.68, 1.40, 0);
					fModLoco.fLocoEngine.setMaxSpeed(1.);
					fback = true;
					pass = true;
					t[126].start();
					}
				if(!t[126].isAlive() && pass2 == false && pass == true)
				{
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(1., 0.5,0);
					flosePoint1[15] = true;
					pass2 = true;
					t[127].start();
				}
				if(!t[127].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}
		}
		mySleep(400);
		
	}
	
	private void Point16() throws InterruptedException, ConnectionLostException
	{
		/**16e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		fModLoco.fLocoEngine.setMaxSpeed(1.);
		fModLoco.setTarget(0., 0., -1.57);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(100);
			
				checkObstacleFront();
		}
		mySleep(400);
	}
	
	private void Point17() throws InterruptedException, ConnectionLostException
	{
		/**17e point*/
		fModLoco.setTarget(1.877, 1.846, 0);
		t[17].start();
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(100);
			
			if (fback == false && flosePoint1[17] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[17] == false))checkObstacleBack();
				
			if(!t[17].isAlive() && pass == false)
			{
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setTarget(1.78, 1.40, 0);
				pass = true;
				fback = true;
				t[128].start();
			}
			if(!t[128].isAlive() && pass2 == false && pass == true)
			{
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				fModLoco.setTarget(1., 0.5,0);
				flosePoint1[17] = true;
				pass2 = true;
				t[129].start();
			}
			if(!t[129].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(400);
	}
	
	private void Point18() throws InterruptedException, ConnectionLostException
	{
		/**18e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		t[18].start();
		fModLoco.fLocoEngine.setUTurnAvoidance(true);
		fModLoco.setTarget(1.78, 1.35, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[18] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[18] == false))checkObstacleBack();
			
			mySleep(100);
			
			if(!t[18].isAlive() && pass == false)
			{
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setTarget(1.78, 1.40, 0);
				pass = true;
				fback = true;
				t[130].start();
			}
				if(!t[130].isAlive() && pass2 == false && pass == true)
				{
					fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
					fModLoco.setTarget(1., 0.5,0);
					flosePoint1[18] = true;
					pass2 = true;
					t[131].start();
					}
				if(!t[131].isAlive() && pass3 == false  && pass2 == true && pass == true)
				{
					fcontinue = true;
				}
		}
		mySleep(400);
	}
	
	private void Point19() throws InterruptedException, ConnectionLostException
	{
		/**19e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		pass3 = false;
		fModLoco.fLocoEngine.setMaxSpeed(1.5);
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.setTarget(1.20, 1.39, 0);
		t[19].start();
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[19] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[19] == false))checkObstacleBack();
			mySleep(300);

			if(!t[19].isAlive() && pass == false)
			{;
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setTarget(1.5, 1.60, 0);
				pass = true;
				fback = true;
				t[132].start();
			}
			if(!t[132].isAlive() && pass2 == false && pass == true)
			{
				fModLoco.setTarget(1.3, 1.4, 0);
				fback = false;
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
				pass2 = true;
				flosePoint1[19] = true;
				t[133].start();
			}
			if(!t[133].isAlive() && pass3 == false  && pass2 == true && pass == true)
			{
				fcontinue = true;
			}
		}
		mySleep(400);
	}
	
	private void Point20() throws InterruptedException, ConnectionLostException
	{
		/**20e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		pass3 =false;
		t[20].start();
		fModLoco.fLocoEngine.setUTurnAvoidance(false);
		fModLoco.fLocoEngine.setMaxSpeed(1.7);
		fModLoco.setTarget(0.773, 1.361, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[20] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[20] == false))checkObstacleBack();
			mySleep(300);
		
			if(!t[20].isAlive() && pass == false)
	   		{	
				fModLoco.setTarget(0.7, 1.65, 0);
				fModLoco.fLocoEngine.setDistancePrecision(0.001);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				t[134].start();
				pass = true;
				fback = true;
			}
		if(!t[134].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.fLocoEngine.setDistancePrecision(0.03);
			fModLoco.setTarget(0.65,1.55,0);
			pass2 = true;
			t[135].start();
			fback = false;
		}
		if(!t[135].isAlive() && pass3 == false  &&  pass2 == true && pass == true)
   		{	
			fModLoco.setTarget(0.7, 1.65, 0);
			fModLoco.fLocoEngine.setDistancePrecision(0.001);
			fModLoco.fLocoEngine.setMaxSpeed(2.);
			fModLoco.fLocoEngine.setUTurnAvoidance(true);
			t[50].start();
			pass3 = true;
			fback = true;
		}
	if(!t[50].isAlive() && pass4 == false  && pass3 == true  &&  pass2 == true && pass == true)
	{
		fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.setTarget(0.65,1.55,0);
		pass3 = true;
		fback = false;
		flosePoint1[20] = true;
		t[51].start();
	}
		if(((!t[51].isAlive() && pass3 == false ) )&&(( pass2 == true )&& (pass == true)))
		{
			fcontinue = true;
		}

		}
		mySleep(200);
	}
	
	private void Point21() throws InterruptedException, ConnectionLostException
	{
		/**21e point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		t[21].start();
		fModLoco.fLocoEngine.setDistancePrecision(0.03);
		fModLoco.fLocoEngine.setMaxSpeed(1.7);
		fModLoco.setTarget(0.655, 1.257, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[21] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[21] == false))checkObstacleBack();
			mySleep(300);
			
			if(!t[21].isAlive() && pass == false)
	   		{	
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				fModLoco.setTarget(0.655, 1.55, 0);
				t[136].start();
				pass = true;
				fback = true;
				
			}
		if(!t[136].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
			fModLoco.setTarget(0.655,1.257,0);
			flosePoint1[21] = true;
			pass2 = true;
			t[137].start();
		}
		if(!t[137].isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fcontinue = true;
		}
		}
		mySleep(400);
	}
	
	private void Point22() throws InterruptedException, ConnectionLostException
	{
		/**22 point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		t[22].start();
		fModLoco.setTarget(0., 0., 1.57);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			mySleep(100);

		}
		mySleep(400);
	}
	
	private void Point23() throws InterruptedException, ConnectionLostException
	{
		/**23 point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;
		t[23].start();
		fModLoco.setTarget(0.64, 1.157, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[23] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[23] == false))checkObstacleBack();
			mySleep(300);
	
			if(!t[23].isAlive() && pass == false)
	   		{	
				fModLoco.setTarget(0.64, 1.55, 0);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				t[140].start();
				pass = true;
				fback = true;
			}
		if(!t[140].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
			fModLoco.setTarget(0.64, 1.157,0);
			flosePoint1[23] = true;
			pass2 = true;
			t[141].start();
		}
		if(!t[141].isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fcontinue = true;
		}
		}
		mySleep(400);
	}
	
	private void Point24() throws InterruptedException, ConnectionLostException
	{
		/**24 point*/
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		t[24].start();
		fModLoco.setTarget(0.332, 0.8, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			if (fback == false && flosePoint1[24] == false ){checkObstacleFront();}
			if ((fback == true)  && (flosePoint1[24] == false))checkObstacleBack();
			mySleep(300);
		
			if(!t[24].isAlive() && pass == false)
	   		{	
				fModLoco.setTarget(0.7, 0.89, 0);
				fModLoco.fLocoEngine.setMaxSpeed(1.);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				//t[142].start();
				pass = true;
				fback = true;
			}
		/*if(!t[142].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
			fModLoco.setTarget(1., 0.5,0);
			flosePoint1[24] = true;
			pass2 = true;
			fback = true;
			t[143].start();
		}
		if(!t[143].isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fcontinue = true;
		}*/
		}
		mySleep(200);
	}
	
	private void Point25() throws InterruptedException, ConnectionLostException
	{
		/**25e point*/ 
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback = false;
		t[25].start();
		fModLoco.fLocoEngine.setUTurnAvoidance(true);
		fModLoco.setTarget(0.7, 0.89,0 );
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{

			checkObstacleBack();
			mySleep(100);
			/*if(!t[25].isAlive() && pass == false)
	   		{	
				fModLoco.setTarget(1.55, 0.25, 0);
				fModLoco.fLocoEngine.setMaxSpeed(1.);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				t[144].start();
				pass = true;
				fback = true;
			}
		if(!t[144].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
			fModLoco.setTarget(1., 0.5,0);
			flosePoint1[5] = true;
			pass2 = true;
			t[145].start();
		}
		if(!t[145].isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fcontinue = true;
		}*/
		}
		mySleep(400);

	}
	
	private void Point26() throws InterruptedException, ConnectionLostException
	{
		/**26e point*/ 
		fcontinue = false;
		pass = false;
		pass2 = false;
		fback=false;	
		//t[26].start();
		fModLoco.setTarget(0.332, 0.8, 0);
		while(fModLoco.fLocoEngine.isTargetReached() != true && fcontinue != true)
		{
			checkObstacleFront();

			mySleep(200);
		}
			/*if(!t[26].isAlive() && pass == false)
	   		{	
				fModLoco.setTarget(1.55, 0.25, 0);
				fModLoco.fLocoEngine.setMaxSpeed(1.);
				fModLoco.fLocoEngine.setUTurnAvoidance(true);
				t[146].start();
				pass = true;
				fback = true;
			}
		if(!t[146].isAlive() && pass2 == false && pass == true)
		{
			fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);
			fModLoco.setTarget(1., 0.5,0);
			flosePoint1[26] = true;
			pass2 = true;
			t[147].start();
		}
		if(!t[147].isAlive() && pass3 == false  && pass2 == true && pass == true)
		{
			fcontinue = true;
		}
		}*/
		//mySleep(200);
	}
	
	public void mylist1() throws InterruptedException, ConnectionLostException
	{
		//Point0();
		Point1();
		
		if(flosePoint1[1]== false )
		{
				Point2();
		}
		
				if(flosePoint1[1]== false && 	flosePoint1[2] == false )
				{
					Point3();	
				}
				
				if((flosePoint1[1]== false) && (flosePoint2[1] == true )) 
				{	
					Point4();
				}
				
				if(flosePoint2[1] == true && flosePoint1[1] == false )
				{
					Point5(); 

				}
				
		
		Point6();Point7();Point8();Point9();Point10();Pointrot();PointAvant_bouteille();Pointrot2();Pointbouteille();Point11();Point12();Point13();
		
		Point19();Point20();
		Point21();	Point22();	Point23();	Point24();
		Point25();Point26();
		/*
		if(flosePoint1[8]==false){Point9();}
		
		

		//Point14();Point15();Point16();
		//Point17();Point18();
		Point19();Point20();
		Point21();	Point22();	Point23();	Point24();
		Point25();Point26();*/
	}
	
	public void gameStarted() throws InterruptedException{
		fModLoco = (ModLoco) fInspector.getObjectByName("world.modloco");
		fSharp10 = (Sharp) fInspector.getObjectByName("world.sharp10");
		fSharp30 = (Sharp) fInspector.getObjectByName("world.sharp30");
		fSharp50 = (Sharp) fInspector.getObjectByName("world.sharp50");
		fSharp70 = (Sharp) fInspector.getObjectByName("world.sharp70");
		fJack = (Jack) fInspector.getObjectByName("world.jack");

		Thread timer90 = new Thread() {
			public void run(){
				//90secondes
				mySleep(88000);
				fModLoco.fLocoEngine.emergencyFreeze();
				//TODO freeze actionneur too
			}
		};
		timer90.start();

		Thread strategy = new Thread() {
			public void run(){
				initConfig();
				
				while(true){ 
							try {
								mylist1();
							} catch (InterruptedException e) {
								// TODO Auto-generated catch block
								e.printStackTrace();
							} catch (ConnectionLostException e) {
								// TODO Auto-generated catch block
								e.printStackTrace();
							}
							
						
						mySleep(88000);
					}

				}
			};
			strategy.start();
		}

	//Methodes des sharps et gamestart
		private void checkObstacleFront() throws InterruptedException, ConnectionLostException {
			if(obstacleDetectFront() )
				fModLoco.setSpeedMotorPid(0., 0., 0.);
			else
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);

		}

		private void checkObstacleBack() throws InterruptedException, ConnectionLostException {
			if(obstacleDetectBack() )
				fModLoco.setSpeedMotorPid(0., 0., 0.);
			else
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);

		}

		private void checkObstacleAll() throws InterruptedException, ConnectionLostException {
			if(obstacleDetectAll() )
				fModLoco.setSpeedMotorPid(0., 0., 0.);
			else
				fModLoco.setSpeedMotorPid(fSpeedP, fSpeedI, fSpeedD);

		}

		private boolean obstacleDetectFront() throws InterruptedException, ConnectionLostException {
			// TODO Auto-generated method stub
			// Lecture des sharps orienté vers l'avant
			if( fSharp70.TestShrap() || fSharp30.TestShrap())
				return true;

			else return false;
		}

		private boolean obstacleDetectBack() throws InterruptedException, ConnectionLostException {
			// TODO Auto-generated method stub
			// Lecture des sharps orienté vers l'arrière
			if( fSharp10.TestShrap() || fSharp50.TestShrap())
				return true;

			else return false;
		}

		private boolean obstacleDetectAll() throws InterruptedException, ConnectionLostException {
			// TODO Auto-generated method stub
			// Lecture de tous les sharps 
			if( fSharp10.TestShrap() || fSharp50.TestShrap()|| fSharp70.TestShrap() || fSharp30.TestShrap() )
				return true;

			else return false;
		}
		
		public boolean onTouch(MotionEvent event) throws InterruptedException{
			this.gameStarted();
			return false;
		}	
	}
